clear all;
close all;

%% initial quadrotor args
m_nominal = 0.65;
g = 9.81;
Ix_nominal=7.5E-3; % Inertia o x axis kg*m^2
Iy_nominal=7.5E-3; % Inertia o y axis kg*m^2
Iz_nominal=1.3E-2; % Inertia o z axis kg*m^2

dx=0; dy=0; dz=0; % disturbances
kx=0.25; ky=0.25; kz=0.25; % air friction

%% continous A, B, C
% States
% x=[x y z vx vy vz roll pitch yaw wr wp wy];

% Control
% u=[T tr tp ty];
A = [0 0 0 1 0 0 0 0 0 0 0 0;
    0 0 0 0 1 0 0 0 0 0 0 0;
    0 0 0 0 0 1 0 0 0 0 0 0;
    0 0 0 -kx/m_nominal 0 0 0 g 0 0 0 0;
    0 0 0 0 -ky/m_nominal 0 -g 0 0 0 0 0;
    0 0 0 0 0 -kz/m_nominal 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 1 0 0;
    0 0 0 0 0 0 0 0 0 0 1 0;
    0 0 0 0 0 0 0 0 0 0 0 1;
    0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0];
B = [0 0 0 0;
    0 0 0 0;
    0 0 0 0;
    0 0 0 0;
    0 0 0 0;
    1/m_nominal 0 0 0;
    0 0 0 0;
    0 0 0 0;
    0 0 0 0;
    0 1/Ix_nominal 0 0;
    0 0 1/Iy_nominal 0;
    0 0 0 1/Iz_nominal];
C = [1 0 0 0 0 0 0 0 0 0 0 0;
    0 1 0 0 0 0 0 0 0 0 0 0;
    0 0 1 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 1 0 0 0];
x_size = 12;
u_size = 4;
y_size = 4;
% 实际用于更新状态的模型
m_update = 0.65;
Ix_update = 7.5E-3;
Iy_update = 7.5E-3;
Iz_update = 1.3E-2;
A_update = [0 0 0 1 0 0 0 0 0 0 0 0;
    0 0 0 0 1 0 0 0 0 0 0 0;
    0 0 0 0 0 1 0 0 0 0 0 0;
    0 0 0 -kx/m_update 0 0 0 g 0 0 0 0;
    0 0 0 0 -ky/m_update 0 -g 0 0 0 0 0;
    0 0 0 0 0 -kz/m_update 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 1 0 0;
    0 0 0 0 0 0 0 0 0 0 1 0;
    0 0 0 0 0 0 0 0 0 0 0 1;
    0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0];
B_update = [0 0 0 0;
    0 0 0 0;
    0 0 0 0;
    0 0 0 0;
    0 0 0 0;
    1/m_update 0 0 0;
    0 0 0 0;
    0 0 0 0;
    0 0 0 0;
    0 1/Ix_update 0 0;
    0 0 1/Iy_update 0;
    0 0 0 1/Iz_update];
C_update = [1 0 0 0 0 0 0 0 0 0 0 0;
    0 1 0 0 0 0 0 0 0 0 0 0;
    0 0 1 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 1 0 0 0];
%% Discrete state-space model
% fs=50; % 100 Hz
fs=5;
Ts=1/fs; 
sysc=ss(A,B,C,0);
sysd=c2d(sysc,Ts);

sysc_update=ss(A_update,B_update,C_update,0);
sysd_update=c2d(sysc_update,Ts);

Am=sysd.A;
Bm=sysd.B;
Cm=sysd.C;
Dm=zeros(x_size,1);

Am_update=sysd_update.A;
Bm_update=sysd_update.B;
Cm_update=sysd_update.C;
Dm_update=zeros(x_size,1);
%% MPC
%% Initzialization
% N_sim=10*fs; %% samples = seconds*frequency
N_sim = 50*fs;

% tunning parameters
Nc=10;  % control horizon
Np=50; % prediction horizon
% R = 0.001;   % control weighting
R=0.5e-4;

% Init control, reference and output signal
u=zeros(u_size,1);  
y=zeros(y_size,1); 

rx=0*ones(1,N_sim);
ry=0*ones(1,N_sim);
rz=0*ones(1,N_sim);
ryaw=0*ones(1,N_sim);

xm_vector=[];
u_vector=[];
deltau_vector=[];
y_vector=[];

% Init system states
xm=zeros(x_size,1); % states vector
Xf=zeros(x_size+y_size,1); % augmented incremetal state [deltax y]'

%% Get the augmented incremental model and the parameters of the incremental trajectory control
[GG, GF, GR, A, B, C , GD, F, G]=mpcgain_mimo(Am,Bm,Cm,Nc,Np,Dm); 

%% Constant part of the incremental trajectory control
f1=GG+R*eye(Nc*u_size,Nc*u_size); % E for the cost function J=xEx'+x'F

%% Constrain matrix M
ymax=[0.8 1 2];  % output max limits (1 x n_y)
ymin=[-0.3 -1 -1.5];  % output min limits (1 x n_y)

% hay que probar
deltaumax=50;
deltaumin=-50;

% para u negativa hay que invertir el motor que aumenta o reduce para
% cambiar el sentido de giro
% u1 hay que limitarlo (dividir /2) porque controla 4 motores en vez de 2
umax=50;
umin=-50;
% ymax;ymin for output number 'n' where G(n,:); -G(n,:); 
%M_output=[G(1,:); -G(1,:); % y1
%            G(3,:); -G(3,:)]; % y3
M_output=[];
[gm,gn]=size(G);
aux=eye(gn);
M_deltau=[aux(1,:);-aux(1,:);
    aux(2,:);-aux(2,:);
    aux(3,:);-aux(3,:);
    aux(4,:);-aux(4,:)];

M_u=[aux(1,:);-aux(1,:);
    aux(2,:);-aux(2,:);
    aux(3,:);-aux(3,:);
    aux(4,:);-aux(4,:)];

M=[M_output; M_deltau; M_u];

%% loop
for k=1:N_sim  
    %% Reference
    rx(k)=sin(0.05*k);    
    ry(k)=cos(0.05*k);       
   rz(k)=0.01*k;    
% rx(k)=1;
% ry(k)=1;
%  rz(k)=1;
    ryaw(k)=45*pi/180;
    
    r=[rx; ry; rz; ryaw];
    
    %% Calculate DeltaU
    % Get the second part of DeltaU    
    f2=GR*r(:,k)-GF*Xf; % F for the cost function J=xEx'+x'F
    DeltaU=f1\f2; % Get DeltaU without constrains
    
    %% Calculate DeltaU with constrains
    % output number 'n' where ymax(n) -ymin(n) -F(n,:) F(n,:)
    %gamma_output=[[ymax(1);-ymin(1)]+[-F(1,:); F(1,:)]*Xf; % y1
    %              [ymax(3);-ymin(3)]+[-F(3,:); F(3,:)]*Xf;]; % y3
    gamma_output=[];
    gamma_deltau=[deltaumax;-deltaumin;
        deltaumax;-deltaumin;
        deltaumax;-deltaumin;
        deltaumax;-deltaumin];
    gamma_u=[umax-u(1);0+u(1);
        umax-u(2);-umin+u(2);
        umax-u(3);-umin+u(3);
        umax-u(4);-umin+u(4)];
    
    gamma=[gamma_output; gamma_deltau; gamma_u];
    
    DeltaU=optim(f1,-f2,M,gamma,DeltaU); %% Get DeltaU with constrains (Hildreth's algorithm)
    
    %% Calculate u
    deltau=DeltaU(1:4);  % Apply the receding control horizon (take only the first element)
    u=u+deltau; % (u(k)=u(k-1)+deltau(k))
    u=u-[m_nominal*g;0;0;0]; % equilibrium point
    %u=[0;0;0;0]; % open-loop simulation
    %% Get the incremental states vector prediction     
    xm_old=xm; 
    %xm=Am*xm+Bm*u; 
    %y=Cm*xm;
    xm = Am_update*xm+Bm_update*u;
    y = Cm_update*xm;

%     % 使用非线性模型计算输出
%     % 初始条件
%     x = xm;
%     step_size = 0.2;
%     end_time = 10;
%     tspan = 0:step_size:end_time;
%     % 使用定步长的4阶龙格库塔求解
%     x = ode4_step(@(t, x) dynamic(tspan, x, u), tspan, x,step_size);
%     xm = x;
%     y = x([1,2,3,9]);
    %y=y+rand()*[0.1;0.2;0.2;0.1]; % distrurbances noise 
    Xf=[xm-xm_old;y]; 
    
    %% add them to vectors
    xm_vector=[xm_vector xm];
    y_vector=[y_vector y];
    u_vector=[u_vector u];
    deltau_vector=[deltau_vector deltau];
  
end

%% Plot
p=0:N_sim-1;
%output and reference
figure
for i=1:4 
    subplot(4,1,i)
    plot(p,y_vector(i,:),'LineWidth', 2,'LineStyle','-');
    hold on
    grid on
    plot(p,r(i,:),'LineStyle','--');
    % add legend
    legend('仿真轨迹','期望轨迹');
end
subplot(4,1,1); xlabel('x (m)');
subplot(4,1,2); xlabel('y (m)');
subplot(4,1,3); xlabel('z (m)');
subplot(4,1,4); xlabel('yaw (rad)');
% reference - output
figure
for i=1:4
    subplot(4,1,i)
    plot(p,r(i,:)-y_vector(i,:),'LineWidth',2)
end
subplot(4,1,1); xlabel('Error in x (m)'); grid on;
subplot(4,1,2); xlabel('Error in y (m)'); grid on;
subplot(4,1,3); xlabel('Error in z (m)'); grid on;
subplot(4,1,4); xlabel('Error in yaw (rad)'); grid on;
% u
% figure
% for i=1:4
%     subplot(4,1,i)
%     plot(p,u_vector(i,:),'LineWidth', 2);
%     xlabel('u'+string(i));
%     grid on
% end

% deltau
% figure
% for i=1:4
%     subplot(4,1,i)
%     plot(p,deltau_vector(i,:),'LineWidth', 2);
%     xlabel('deltau'+string(i));
%     grid on
% end

% xm
% figure
% for i=1:12
%     subplot(4,3,i)
%     plot(p,xm_vector(i,:));
%     hold on
%     grid on
%     xlabel('x'+string(i));    
% end
% 
% figure
% plot3(y_vector(1,:),y_vector(2,:),y_vector(3,:),'LineWidth', 2,'LineStyle','-');
% hold on
% grid on
% plot3(r(1,:),r(2,:),r(3,:),'LineStyle','--')
% legend('实际轨迹','期望轨迹')
% xlabel('x (m)');ylabel('y (m)');zlabel('z (m)');